/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: flyto.cpp,v 1.1 2004/09/12 03:40:40 poine Exp $
 *
 * (c) 2003 Trammell Hudson <hudson@rotomotion.com>
 *
 * Simple program to send/retrieve a waypoint from the helicopter.
 *
 *************
 *
 *  This file is part of the autopilot simulation package.
 *
 *  Autopilot is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Autopilot is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Autopilot; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <unistd.h>

#include "macros.h"
#include "timer.h"
#include "byteorder.h"
#include <getoptions/getoptions.h>
#include <state/Server.h>
#include <mat/Conversions.h>
#include <mat/Nav.h>

using namespace libstate;
using namespace libmat;
using namespace util;
using namespace std;


static double		north;
static double		east;
static double		down;
static double		hdg;
static int		received_ok;
static int		verbose;

static int
help( void )
{
	cerr <<
"Usage: flyto [options] north east down [heading]\n"
"\n"
"	-h | --help			This help\n"
"	-v | --verbose			Increase verbosity\n"
"	-V | --version			Version\n"
"	-s | --server hostname		State server\n"
"	-p | --port port		State port\n"
"	-r | --relative			Relative to the current position\n"
"\n"
"The north, east and down are in meters absolute in the local tangent\n"
"plane or relative to the current position.\n"
"\n"
"Heading is in degrees from north (both -180 to +180 or 0 to 360\n"
"are supported.  If it is unspecified, then the current heading will\n"
"be maintained.\n"
	<< endl;

	return -10;
}

static int
version( void )
{
	cerr << "$Id: flyto.cpp,v 1.1 2004/09/12 03:40:40 poine Exp $" << endl;
	return -10;
}


static void
process_desired(
	void *			UNUSED( priv ),
	const Host_t *		src,
	int			UNUSED( type ),
	const timeval *		UNUSED( when ),
	const void *		data,
	size_t			len
)
{
	msg_desired_pos_t *	msg = (msg_desired_pos_t*) data;

	if( len != sizeof( *msg ) )
	{
		cerr << "Invalid desired packet from " << src << endl;
		return;
	}

	double	desired_north	= ntohf( msg->n );
	double	desired_east	= ntohf( msg->e );
	double	desired_down	= ntohf( msg->d );
	double	desired_hdg	= ntohf( msg->hdg );

	if( verbose )
		printf( "Desired state:"
			" % 8.4f % 8.4f % 8.4f @ % 8.4f"
			"\n",
			desired_north,
			desired_east,
			desired_down,
			desired_hdg
		);

	if( desired_north == north
	&&  desired_east  == east
	&&  desired_down  == down
	&&  desired_hdg   == hdg
	) {
		received_ok++;

		printf( "Received ACK from flight controller\n" );
	}
}

int
main(
	int			argc,
	char **			argv
)
{
	const char *		program		= argv[0];
	const char *		server_host	= "127.0.0.1";
	int			server_port	= 2002;
	int			relative	= 0;
	int			got_hdg		= 0;


	int rc = getoptions( &argc, &argv,
		"h|?|help&",		help,
		"V|version&",		version,
		"v|verbose+",		&verbose,
		"s|server=s",		&server_host,
		"p|port=i",		&server_port,
		"r|relative+",		&relative,
		0
	);

	if( rc == -10 )
		return EXIT_FAILURE;
	if( rc < 0 )
		return help();

	if( argc < 3 || argc > 4)
		return help();

	north	= atof( argv[0] );
	east	= atof( argv[1] );
	down	= atof( argv[2] );

	if( argc == 4 )
	{
		hdg	= atof( argv[3] ) * C_DEG2RAD;
		got_hdg	= 1;
	}


	if( verbose > 1 )
		cout
			<< "Connecting to "
			<< server_host
			<< ":"
			<< server_port
			<< endl;

	state_t			state;
	Server			server( server_host, server_port );

	server.handle( AHRS_STATE, Server::process_ahrs, (void*) &state );
	server.handle( DESIRED_POSITION, process_desired, 0 );

	while( 1 )
	{

		int type = server.get_packet();

		if( type < 0 )
		{
			cout << "Read error from server" << endl;
			return -1;
		}

		if( type != AHRS_STATE )
		{
			if( verbose > 1 )
				cout << "Got packet of type " << type << endl;
			continue;
		}

		break;
	}

	if( verbose )
		printf( "AHRS state:"
			" % 8.4f % 8.4f % 8.4f @ % 8.4f"
			"\n",
			state.x,
			state.y,
			state.z,
			state.psi
		);


	/*
	 * Compute and send our new desired positon.
	 */
	msg_desired_pos_t	msg;

	if( relative )
	{
		north	+= state.x;
		east	+= state.y;
		down	+= state.z;
		hdg	+= state.psi;
	}

	/* Use the current heading if they did not specify one */
	if( !got_hdg )
		hdg	= state.psi;

	/* Convert the heading into -Pi..+Pi */
	if( hdg < -C_PI )
		hdg += 2 * C_PI;
	if( hdg >  C_PI )
		hdg -= 2 * C_PI;

	msg.n	= htonf( north );
	msg.e	= htonf( east );
	msg.d	= htonf( down );
	msg.hdg	= htonf( hdg );

	printf( "Sending desired state:"
		" % 8.4f % 8.4f % 8.4f @ % 8.4f"
		"\n",
		north,
		east,
		down,
		hdg
	);

	server.send_packet(
		DESIRED_POSITION,
		(void*) &msg,
		sizeof( msg )
	);


	/*
	 * Wait for an ack
	 */
	int packet_count = 0;

	while( ++packet_count < 100 )
	{
		int type = server.get_packet();

		if( type < 0 )
		{
			cerr << "Read error from server" << endl;
			return -1;
		}

		if( type != DESIRED_POSITION )
		{
			if( verbose > 1 )
				cout << "Got packet of type " << type << endl;
			continue;
		}

		if( received_ok )
			return 0;
	}

	/* Did not receive an acceptable ACK */
	cerr << "No ACK received" << endl;
	return -1;
}
